/*
 * tarm_runBase.c
 *
 * function: normal run function
 *  s作用：普通运行函数，没有加减速功能
 *
 *  Created on: 2020年2月29日
 *      Author: AN
 */

#include <stdlib.h>

#include "middleware/tarm_tim.h"
#include "middleware/tarm_math.h"
#include "middleware/tarm_uart.h"

#include "hardware/tarm_io.h"
#include "hardware/tarm_limit.h"
#include "hardware/tarm_pwm.h"
#include "hardware/tarm_baseTimer.h"

#include "application/tarm_init.h"
#include "application/tarm_run.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

#include "hardware/closeloop/CloseLoop.h"

/************************************************************
*	已知xy，调用函数即可驱动机械臂运动至目标位置
*	传入参数：	x（x的值）；y（y的值）；speed_ms（速度的倒数，ms/cm）
*	返回值：	-1（出错）；0（成功）
*	运动范围：以(20,0)为圆心，15cm为半径 + >=-10cm
*	注释：	如果传入的xy超过范围，将返回-1，g_TARM_RunRecord.pv_distance.x不变
			speed方面，已经通过控制分频系数控制时间单位为100us，
			实验方面确定1500us比较合适；
************************************************************/
int  point_to_point_xy(double x,double y,uint16_t speed_ms)
{
	double distance,time;
	double prime_time,vice_time;

	//1、判断x、y是否超过界限
	if(y >= -10){
			double counter = (x-20)*(x-20) + y*y;
			if(counter >= 992){
					return -1;
			}
	}
	else
			return -1;

	//2、如果正确，修改x、y的值
	g_TARM_RunRecord.pv_distance.x = x;
	g_TARM_RunRecord.pv_distance.y = y;

	//3、调用xy_des_to_angle自动同步目标【xy-角度关系】
	xy_des_to_angle();

	//4、计算对应速度speed_ms的对应时间（以speed_ms这个速度需要多少时间time）
//	distance = sqrt((g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)*(g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)
//									+ (g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y)*(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y));
	distance = tarm_sqrt((g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)*(g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)
									+ (g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y)*(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y));
	time = distance*speed_ms;		//单位：ms

	//5、计算对应每步速度（每步多少ms）,*10因为定时器启动的是100us
	//uint16_t prime_time,vice_time;
	prime_time = (g_TARM_Factor.step_p*time*10)/abs(g_TARM_RunRecord.pv_current.p - g_TARM_RunRecord.pv_distance.p);
	vice_time = (g_TARM_Factor.step_v*time*10)/abs(g_TARM_RunRecord.pv_current.v - g_TARM_RunRecord.pv_distance.v);


	//5、计算重装载值、分频系数，初始化定时器
	if(prime_time<2)
		prime_time = 2;
	if(vice_time<2)
		vice_time = 2;
//	prime_time = prime_time/10;
//	vice_time = vice_time/10;

	//6、同步时间+启动定时器
	g_TARM_RunRecord.CounterTimer_p = prime_time;
	g_TARM_RunRecord.CounterTimer_v = vice_time;

	g_TARM_RunRecord.RunFlag_p = 1;
	g_TARM_RunRecord.RunFlag_v = 1;

	TimeBase_Reset();
	TimeBase_Enable();

	return 0;
}


/************************************************************
*	已知xyr，调用函数即可驱动机械臂运动至目标位置
*	传入参数：	x（x的值）；y（y的值）；r（rotation的值）；
	speed_ms（速度的倒数，毫秒ms值）
*	返回值：	-1（出错）；0（成功）
*	运动范围：以(20,0)为圆心，15cm为半径 + >=-10cm
*	注释：	如果传入的xy超过范围，将返回-1，g_TARM_RunRecord.pv_distance.x不变
			speed方面，已经通过控制分频系数控制时间单位为100us，
			实验方面确定1500us比较合适；
************************************************************/
int  point_to_point_xyr(double x,double y,double r,uint16_t speed_ms)
{
	double distance,time;

	//3、计算对应速度speed_ms的对应时间（以speed_ms这个速度需要多少时间time）
	distance = sqrt((g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)*(g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)
				+ (g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y)*(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y));
	time = distance*speed_ms;		//单位：/100ms

	if(point_to_point_xy(x,y,speed_ms)==0)
		point_to_point_rotation(r,time);

	return 0;
}

/************************************************************
*	已知rotation，调用函数即可驱动机械臂运动至目标位置
*	传入参数：	r（rotation的值）；speed_ms（速度的倒数，毫秒ms/度）
*	返回值：	-1（出错）；0（成功）
*	注释：	如果传入的xy超过范围，将返回-1，g_TARM_RunRecord.pv_distance.x不变
			speed方面，已经通过控制分频系数控制时间单位为100us，
			实验方面确定1500us比较合适；
************************************************************/
int  point_to_point_rotation(double r,uint16_t speed)
{
	uint16_t r_time;

	//1.1、转换r为栅格中的标准值
	r = ((int)(r/g_TARM_Factor.step_p))*g_TARM_Factor.step_p;

	//2、修改rotation的值
	g_TARM_RunRecord.r_val.d = r;
	g_TARM_RunRecord.r_val.c = 0;

	//4、计算对应每步速度（每步多少ms）
	r_time = (g_TARM_Factor.step_r*speed)/abs(g_TARM_RunRecord.r_val.d);

	//5、计算重装载值、分频系数，初始化定时器
//	r_time = r_time/10;
	if(r_time<2)
		r_time = 2;

	//6、同步时间+启动定时器
	g_TARM_RunRecord.CounterTimer_r = r_time;

	g_TARM_RunRecord.RunFlag_r = 1;

	TimeBase_Enable();

	return 0;
}


/************************************************************
*	已知angle，调用函数即可驱动机械臂运动至目标位置
*	传入参数：	prime（x轴的值）；vice（y轴的值）；speed_ms（速度的倒数，毫秒ms/cm）
*	返回值：	-1（出错）；0（成功）
*	运动范围：以(20,0)为圆心，15cm为半径 + >=-10cm
*	注释：	如果传入的angle值转换成xy后发现超过范围，将返回-1；
			包括g_TARM_RunRecord.pv_distance.x、prime等值都不变
************************************************************/
int point_to_point_angle(double prime,double vice,uint32_t speed_ms)
{
	double distance,time;
	uint16_t prime_time,vice_time;

	//1.1、转换prime、vice为栅格中的标准值
	double x,y;
	int prime_int,vice_int;

	prime_int = ((int)((prime - g_TARM_Factor.null_p)/g_TARM_Factor.step_p));
	vice_int = ((int)((vice - g_TARM_Factor.null_v)/g_TARM_Factor.step_v));

	prime = g_TARM_Factor.null_p + prime_int*g_TARM_Factor.step_p;
	vice = g_TARM_Factor.null_v + vice_int*g_TARM_Factor.step_v;

	//1.2、计算对应xy
	angle_to_xy(prime,vice,&x,&y);

	//1.3、判断x、y是否超过界限
	if(y >= -10){
		double counter = (x-20)*(x-20) + y*y;
		if(counter >= 992){
			return -1;
		}
	}
	else
		return -1;

	//2、如果正确，修改x、y的值
	g_TARM_RunRecord.pv_distance.x = x;
	g_TARM_RunRecord.pv_distance.y = y;

	//3、修改g_TARM_RunRecord.pv_distance.p、g_TARM_RunRecord.pv_distance.v的值/调angle_des_to_xy（不过这样重复计算，浪费资源）
	//自动同步目标【xy-角度关系】
	g_TARM_RunRecord.pv_distance.p = prime;
	g_TARM_RunRecord.pv_distance.v = vice;

	//4、计算对应速度speed_ms的对应时间（以speed_ms这个速度需要多少时间time）
	distance = sqrt((g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)*(g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x) + (g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y)*(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y));
//	distance = tarm_sqrt((g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)*(g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x) + (g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y)*(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y));
	time = distance*speed_ms;		//单位：ms

	//5、计算对应每步速度（每步多少ms）,*10因为定时器启动的是100us
	prime_time = ((double)g_TARM_Factor.step_p*(double)time*10)/abs(g_TARM_RunRecord.pv_current.p - g_TARM_RunRecord.pv_distance.p);
	vice_time = ((double)g_TARM_Factor.step_v*(double)time*10)/abs(g_TARM_RunRecord.pv_current.v - g_TARM_RunRecord.pv_distance.v);

	//5、计算重装载值、分频系数，初始化定时器
	if(prime_time<2)
		prime_time = 2;
	if(vice_time<2)
		vice_time = 2;
//	prime_time = prime_time/10;
//	vice_time = vice_time/10;

	//6、同步时间+启动定时器
	g_TARM_RunRecord.CounterTimer_p = prime_time;
	g_TARM_RunRecord.CounterTimer_v = vice_time;

	g_TARM_RunRecord.RunFlag_p = 1;
	g_TARM_RunRecord.RunFlag_v = 1;

	TimeBase_Enable();

	return 0;
}
/************************************************************
*	只使用一个定时器产生时基，在定时器中断函数中调用的处理函数
*	handle_prime：x轴，
*	handle_vice：y轴，
************************************************************/
void handle_interrupt(void)
{
	static uint8_t prime_counter=0,vice_counter=0,rotation_counter=0;

	//0、把所有STP IO拉低
	X_STP_RESET;
	Y_STP_RESET;
	Z_STP_RESET;

	//1、判断flag，让所有counter++
	if(g_TARM_RunRecord.RunFlag_p)
		prime_counter++;
	if(g_TARM_RunRecord.RunFlag_v)
		vice_counter++;
	if(g_TARM_RunRecord.RunFlag_r)
		rotation_counter++;

	//2、判断时间，到达则调用对应handle函数，counter清零（每个各自的handle里面会处理run_flag清零的事情）
	if(g_TARM_RunRecord.RunFlag_p && prime_counter >= g_TARM_RunRecord.CounterTimer_p){
		handle_prime();
		prime_counter = 0;
	}

	if(g_TARM_RunRecord.RunFlag_v && vice_counter >= g_TARM_RunRecord.CounterTimer_v){
		handle_vice();
		vice_counter = 0;
	}

	if(g_TARM_RunRecord.RunFlag_r && rotation_counter >= g_TARM_RunRecord.CounterTimer_r){
		handle_rotation();
		rotation_counter = 0;
	}


	//3、判断每个flag是否为零，全为零清除中断
	if(!(g_TARM_RunRecord.RunFlag_p||g_TARM_RunRecord.RunFlag_v||g_TARM_RunRecord.RunFlag_r))
	{
		TimeBase_Disable();
		TimeBase_ClearFlag();  //清除TIMx更新中断标志

		if(InitFlag==1 && CloseLoopSingle.CloseLoopFlag!=Open_CloseLoop)	//初始化完成后才闭环，非必须是CloseLoopFlag才闭环
			CloseLoopUpdate();

		ReportStruct.Uart1ReportFlag.ExecuteFinish = 1;
		ReportStruct.Uart2ReportFlag.ExecuteFinish = 1;	//上报
	}
}


/************************************************************
*	在定时器中断函数中调用的处理函数
*	handle_prime：x轴，TIM2
*	handle_vice：y轴，TIM3
************************************************************/
//prime（x轴）轮询处理函数（以角度值为基准）
void handle_prime(void)
{
	//1.1、判断方向
	if(g_TARM_RunRecord.pv_distance.p<g_TARM_RunRecord.pv_current.p)
		X_DIR_RESET;
	else
		X_DIR_SET;

	//1.2、脉冲步进
	X_STP_SET;
	//delay_us(1);
	//X_STP_RESET;

	//2、更新角度目前数值
	if(g_TARM_RunRecord.pv_distance.p>g_TARM_RunRecord.pv_current.p)
		g_TARM_RunRecord.pv_current.p += g_TARM_Factor.step_p;
	else
		g_TARM_RunRecord.pv_current.p -= g_TARM_Factor.step_p;

	//3、更新目前x数值
	angle_to_xy(g_TARM_RunRecord.pv_current.p,g_TARM_RunRecord.pv_current.v,&g_TARM_RunRecord.pv_current.x,&g_TARM_RunRecord.pv_current.y);

	//4、判断是否到达，到达则关闭定时器（使用角度值比较）
	if(fabs(g_TARM_RunRecord.pv_current.p - g_TARM_RunRecord.pv_distance.p) <= 0.02)	prime_arrive();
}

//vice（y轴）轮询处理函数
void handle_vice(void)
{
	//1.1、判断方向
	if(g_TARM_RunRecord.pv_distance.v>g_TARM_RunRecord.pv_current.v)
		Y_DIR_SET;
	else
		Y_DIR_RESET;

	//1.2、脉冲步进
	Y_STP_SET;
	//delay_us(1);
	//Y_STP_RESET;

	//2、更新角度目前数值
	if(g_TARM_RunRecord.pv_distance.v>g_TARM_RunRecord.pv_current.v)
		g_TARM_RunRecord.pv_current.v += g_TARM_Factor.step_v;
	else
		g_TARM_RunRecord.pv_current.v -= g_TARM_Factor.step_v;

	//3、更新目前y数值
	angle_to_xy(g_TARM_RunRecord.pv_current.p,g_TARM_RunRecord.pv_current.v,&g_TARM_RunRecord.pv_current.x,&g_TARM_RunRecord.pv_current.y);

	//4、判断是否到达，到达则关闭定时器（使用角度值比较）
	if(fabs(g_TARM_RunRecord.pv_current.v - g_TARM_RunRecord.pv_distance.v) <= 0.02)	vice_arrive();
}

void handle_rotation(void)
{
	//1.1、判断方向
	if(g_TARM_RunRecord.r_val.d>0)
		Z_DIR_SET;
	else
		Z_DIR_RESET;

	//1.2、脉冲步进
	Z_STP_SET;
	//delay_us(1);
	//Z_STP_RESET;

	//2、更新角度目前数值
	g_TARM_RunRecord.r_val.c += g_TARM_Factor.step_r;

	//3、判断是否到达，到达则关闭定时器（使用角度值比较）
	if(fabs(fabs(g_TARM_RunRecord.r_val.c) - fabs(g_TARM_RunRecord.r_val.d)) <= 0.02)
		rotation_arrive();
}


/************************************************************
*	目标点到达处理
************************************************************/
void prime_arrive(void)
{
	g_TARM_RunRecord.RunFlag_p = 0;
	PRINT_MAIN("\nPR-AR\n");
//	PRINT_MAIN("\n 开环结果：\tprime_arrive:%f-%f\r\n",g_TARM_RunRecord.pv_current.p,g_TARM_RunRecord.pv_distance.p);
//	PRINT_MAIN("\t\tnow_x:%f;now_y:%f\r\n",g_TARM_RunRecord.pv_current.x,g_TARM_RunRecord.pv_current.y);
}

void vice_arrive(void)
{
	g_TARM_RunRecord.RunFlag_v = 0;
	PRINT_MAIN("\nVI-AR\n");
//	PRINT_MAIN("\n 开环结果：\tvice_arrive:%f-%f\r\n",g_TARM_RunRecord.pv_current.v,g_TARM_RunRecord.pv_distance.v);
//	PRINT_MAIN("\t\tnow_x:%f;now_y:%f\r\n",g_TARM_RunRecord.pv_current.x,g_TARM_RunRecord.pv_current.y);
}

void rotation_arrive(void)
{
	g_TARM_RunRecord.RunFlag_r = 0;
	PRINT_MAIN("\nRO-AR\n");
//	PRINT_MAIN("\n rotation arrive \r\n");
}

